# -*- coding: utf-8 -*-
from naoqi import *
import time
import math
import time
import almath
from naoqi import ALProxy

robotIP="192.168.1.14"
PORT=9559
'''ALM = ALProxy("ALMotion", robotIP, PORT)
ALP = ALProxy("ALRobotPosture", robotIP, PORT)
ALM.wakeUp()'''

a1=True
a2=False
a3=True

motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

motionProxy.wakeUp()
postureProxy.goToPosture("StandInit", 0.5)

motionProxy.moveTo(0, 0, 0)
y=0

if (a1 ==True):
    y=1
    # t=-1*math.pi/4
    t=0
    motionProxy.moveTo(1.7, y, t)

    # 锁定头部，让其低头
    #ALM.angleInterpolation("HeadYaw", -0.00, 1, False)

    #ALM.angleInterpolation("HeadPitch", 0.5, 1, False)
    #ALM.angleInterpolation('HeadYaw', -30 * almath.TO_RAD, 0.2)

    # 头部左转30°

    i1=0
elif(a1 ==False):
    y = -0.6
    # t = math.pi / 4
    t=0
    motionProxy.moveTo(1, y, t),


    i1=1


if(i1==0 and a2 == True):
    y=0
    t =0
    i2=0
    motionProxy.moveTo(1, y, t)


elif(i1==1 and a2 == True):
    y=0.6
    t = 0
    i2 = 0
    motionProxy.moveTo(1, y, t)


elif(i1==0 and a2 == False):
    y=-0.35
    t = 0
    i2 = 1
    motionProxy.moveTo(0, -0.5, t)
    motionProxy.moveTo(0.75, y, t)


elif(i1 == 1 and a2 == False):
    y = 0
    t = 0
    i2 = 1
    motionProxy.moveTo(1, y, t)


if(i2==0 and a3 == True):
    y=0
    t = 0
    i3=0
    motionProxy.moveTo(1, y, t)


elif(i2==1 and a3 == True):
    y =1.15
    t =0
    i3 = 0
    motionProxy.moveTo(0, 0.40, t)
    #time.sleep(0.10)
    motionProxy.moveTo(0.8, y, t)
    #time.sleep(0.10)
    motionProxy.moveTo(1, 0, t)
    #ALM.angleInterpolation("HeadYaw", -0.00, 1, False)

    #ALM.angleInterpolation('HeadYaw', 30 * almath.TO_RAD, 0.2)
elif(i2==0 and a3 == False):
    y=-1
    t = 0
    i3 = 1
    motionProxy.moveTo(1, y, t)
    motionProxy.moveTo(1, 0, t)
    time.sleep(3)


elif(i2 == 1 and a3 == False):
    y = 0
    t = 0
    i3 = 1
    motionProxy.moveTo(1, y, t)

